/*
 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
 */

#include <stdio.h>
#include <string.h>
#include <stdint.h>

#include "aos/hal/can.h"
#include "board.h"
#include "ulog/ulog.h"

#include "canbus.h"
#include "aos/list.h"


static canbus_t g_canbus;
static aos_queue_t can_rx_queue;

#define CAN_RX_BUFFER_SIZE 16
static can_dev_t can_dev_instanse = {PORT_CAN_RESERVED, {250000, 0, 0, 0}, NULL};

static void canio_rx_isr(can_frame_t *can_frame_rx, void* para);

/* can filter list for master */
static can_filter_item_t can_filter_items[] =
{
    {0, 0x80, 0x780},
    {0, 0x180, 0x780},
    {0, 0x280, 0x780},
    {0, 0x380, 0x780},
    {0, 0x480, 0x780},
    {0, 0x580, 0x780},
    {0, 0x700, 0x780},
};

#ifdef LOG_TAG
#undef LOG_TAG
#endif

#define LOG_TAG "CAN_DEMO"

void *canbus_mutex_new(void)
{
    aos_mutex_t mutex;
    if (0 != aos_mutex_new(&mutex)) {
        return NULL;
    }

    return mutex.hdl;
}

void canbus_mutex_free(void *mutex)
{
    if (NULL != mutex) {
        aos_mutex_free((aos_mutex_t *)&mutex);
    }
}

void canbus_mutex_lock(void *mutex)
{
    if (NULL != mutex) {
        aos_mutex_lock((aos_mutex_t *)&mutex, AOS_WAIT_FOREVER);
    }
}

void canbus_mutex_unlock(void *mutex)
{
    if (NULL != mutex) {
        aos_mutex_unlock((aos_mutex_t *)&mutex);
    }
}

void *canbus_sem_new(void)
{
    aos_sem_t sem;

    if (0 != aos_sem_new(&sem, 0)) {
        return NULL;
    }

    return sem.hdl;
}

void canbus_sem_free(void *sem)
{
    aos_sem_free((aos_sem_t *)&sem);
}

void canbus_sem_signal(void *sem)
{
    aos_sem_signal((aos_sem_t *)&sem);
}

int canbus_sem_wait(void *sem, uint32_t timeout_ms)
{
    return aos_sem_wait((aos_sem_t *)&sem, timeout_ms);
}

static void canio_rx_isr(can_frame_t *can_frame_rx, void* para)
{
    if (NULL != can_frame_rx && NULL != para) {
        aos_queue_t* queue = (aos_queue_t*)para;
        aos_queue_send(queue, can_frame_rx, sizeof(can_frame_t)-(8-can_frame_rx->header.dlc));
    }
}

static int32_t normal_can_rx(can_frameheader_t *rxheader, void *data, const uint32_t timeout)
{
    int32_t rc = -1;
    can_frame_t can_frame;
    int32_t size = 0;
	aos_queue_t *p_rx_queue = &g_canbus.can_rx_queue;
	
    rc = aos_queue_recv(&can_rx_queue, timeout, &can_frame, &size);
    if (0 == rc) {
        memcpy(rxheader, &can_frame.header, sizeof(can_frameheader_t));
        if (can_frame.header.dlc > 0 && can_frame.header.rtr == 0 && data != NULL) {
            memcpy(data, &can_frame.data, can_frame.header.dlc);
        }
    }
    return rc;
}

static int lidar_status = 0;
static void can_demo_routine(void *param)
{
    can_frameheader_t header;
	canbus_t     *bus = &g_canbus;
	canbus_dev_t *dev = NULL;
	slist_t      *cur = NULL;

	
    uint8_t data[8];
	int i = 0;
    while(1) {

        if(0 == hal_can_recv(&can_dev_instanse, &header, data, AOS_WAIT_FOREVER) ) {
            can_frame_t tx_frame;
			//LOGI(LOG_TAG, "Rx CAN ID 0x%x(%d), RTR:%d ",header.id, header.dlc, header.rtr);

			slist_for_each_entry_safe(&bus->bus_l, cur, dev, canbus_dev_t, next) {
		    if (dev->rx_id == header.id)
		        break;
		    }

			if (dev == NULL)
			{
				LOGE(LOG_TAG, "can t find the dev is 0x%x", header.id);
				continue;
			}
            
			
			if (dev->sem_t != NULL) {
				memcpy(dev->rx_buf, data, header.dlc);
				canbus_sem_signal(dev->sem_t);
			}			
        }
    }
}

static int32_t can_send(const can_frame_t *frame)
{
    int32_t rc = -1;
	canbus_t *bus = &g_canbus;
	
    if (NULL != frame) {
		canbus_mutex_lock(&bus->send_mutex);
        rc = hal_can_send(&can_dev_instanse, &frame->header, frame->data, CAN_SEND_BLOCK_MS);
		canbus_mutex_unlock(&bus->send_mutex);
    } else {
        LOGE(LOG_TAG, "invalid param");
    }

    return rc;
}

static can_frame_t can_rx_buf[CAN_RX_BUFFER_SIZE];

int32_t canbus_init(void)
{
    int32_t rc = -1;

	canbus_t *bus = &g_canbus;
	aos_queue_t *p_rx_queue = &bus->can_rx_queue;

	bus->can_rx_pbuf = (can_frame_t *)aos_malloc(sizeof(can_frame_t) * CAN_RX_BUFFER_SIZE);
	if (bus->can_rx_pbuf == NULL) {
		return -1;
	}

	bus->id = can_dev_instanse.port;
	bus->dev_num = 0;
	bus->canbus_send = can_send;
	slist_init(&bus->bus_l);

	bus->bus_mutex = canbus_mutex_new();
    if (NULL == bus->bus_mutex) {
        LOGE(LOG_TAG, "create bus mutex failed\r\n");
        goto err;
    }

	bus->send_mutex = canbus_mutex_new();
    if (NULL == bus->send_mutex) {
        LOGE(LOG_TAG, "create send mutex failed\r\n");
        goto err;
    }
	
    if (0 == aos_queue_new(&can_rx_queue, can_rx_buf , sizeof(can_rx_buf), CAN_RX_BUFFER_SIZE)) {
        if (0 != hal_can_init(&can_dev_instanse)) {
            LOGE(LOG_TAG, "Init can Fail");
        } else {
            can_mapping_t *can_mapping = can_mapping_node(can_dev_instanse.port);

            if (NULL != can_mapping) {
                can_mapping->isr_rx_handler = canio_rx_isr;
                can_mapping->customer_recv  = normal_can_rx;
                can_mapping->isr_rx_para    = &can_rx_queue;

                if (0 != hal_can_filter_init(&can_dev_instanse, sizeof(can_filter_items) / sizeof(can_filter_items[0]), can_filter_items)) {
                    LOGE(LOG_TAG, "Set CAN Filter Failed");
                } else {
                    if (0 != hal_can_start(&can_dev_instanse)) {
                        LOGE(LOG_TAG, "Start CAN Failed");
                    } else {
                        LOGI(LOG_TAG, "Start can sucessfully");
                        if (0 != aos_task_new("can_demo", can_demo_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
                            LOGE(LOG_TAG, "create routine fail");
                        }
                        rc = 0;
                    }
                }
            } else {
                LOGE(LOG_TAG, "Over Write can Fail");
            }
        }
    } else {
        LOGE(LOG_TAG, "Init CAN Rx MessageQ Failed");
    }

	return 0;
	err:
    canbus_dinit();

    return rc;
}

int32_t canbus_dinit(void)
{
	return 0;
}

int canbus_dev_register(canbus_dev_t *dev)
{
	canbus_t *bus = &g_canbus;
    if (NULL == dev) {
        LOGE(LOG_TAG, "invalid input %s \r\n", __func__);
        return -1;
    }

	dev->bus = bus;
	dev->sem_t = canbus_sem_new();
	printf("canbus register id :0x%x\r\n", dev->rx_id);
    if (NULL == dev->sem_t) {
        LOGE(LOG_TAG, "create dev sem failed\r\n");
        return -1;
    }

    canbus_mutex_lock(bus->bus_mutex);
    slist_add_tail(&dev->next, &bus->bus_l);
    canbus_mutex_unlock(bus->bus_mutex);

    return 0;
}

int canbus_dev_unregister(canbus_dev_t *dev)
{
	canbus_t *bus = &g_canbus;
    if (NULL == dev) {
        LOGE(LOG_TAG, "invalid input %s \r\n", __func__);
        return -1;
    }

	dev->bus = NULL;
	if (dev->sem_t != NULL) {
		canbus_sem_free(dev->sem_t);
	}
	
    canbus_mutex_lock(bus->bus_mutex);
    slist_del(&dev->next, &bus->bus_l);
    canbus_mutex_unlock(bus->bus_mutex);

    return 0;
}



